#include <Servo.h> //needed for the arm library
#include <Arm.h>
#include <Robot_CLI.h>

//Robot_CLI cli;
Arm arm;
Robot_CLI cli;
double tcp[ 3 ];
int command[ 8 ];

void setup()
{
  arm.initArm();    //Initialize the arm.
  cli.startComm();  //Start com with default speed 9600.
  
  arm.goToCal();    //Send arm to calibration position.
  delay( 1000 );    //Give the arm a chance to get there.
}

void loop()
{
  if( 0 == cli.getCommand( command, 8 ))
  {
    switch( command[ 0 ] )
    {
      case 'a':
         arm.setAxisAngle( command[ 1 ], command[ 2 ] );
         break;
      case 'g':
         if( command[ 1 ] == 1 && command[ 2 ] == 0 )
         {
           arm.gripperClose(); 
         }
         else if( command[ 1 ] == 1 && command[ 2 ] == 0 )
         {
           arm.gripperOpen(); 
         }
         break;
      case 'i':
         arm.setTCP( command[ 1 ], command[ 2 ], command[ 3 ], 0 ); //Need to fix this function
                                                                   //in the arm lib.
         break;
    }
  } 
}

/*
  a,axis,angle,0,0,0,0,0"\n"  where a is a command identifier
			      axis is a integer 1 through 5
			      and angle is an integer 0 to 180
  g,open,close,0,0,0,0,0"\n"  where open and close are either 1 or 0
			      in order to open the gripper you need the
			      command g,1,0,0,0,0,0,0"\n" and in order 
			      to close you need g,0,1,0,0,0,0,0"\n"
  i,x,y,z,0,0,0,0"\n"	      where x, y, and z are integers inside the
			      allowed range for the robot travel.
*/
